#include "motor.h"

/*
https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-atmega328

// CS BITS
CS02 CS01 CS00 DESCRIPTION
 0    0    0    Timer/Counter0 Disabled 
 0    0    1    No Prescaling
 0    1    0    Clock / 8
 0    1    1    Clock / 64
 1    0    0    Clock / 256
 1    0    1    Clock / 1024

CS12	 CS11 	 CS10 	 DESCRIPTION
0	0 	0 	 Timer/Counter1 Disabled 
0	0 	1 	 No Prescaling
0	1 	0 	 Clock / 8
0	1 	1 	 Clock / 64
1	0 	0 	 Clock / 256
1	0 	1 	 Clock / 1024

CS22	 CS21 	 CS20 	 DESCRIPTION
0	0 	0 	 Timer/Counter2 Disabled 
0	0 	1 	 No Prescaling
0	1 	0 	 Clock / 8
0	1 	1 	 Clock / 32
1	0 	0 	 Clock / 64
1	0 	1 	 Clock / 128
1	1 	0 	 Clock / 256
1	1 	1 	 Clock / 1024 


// WAVEFORM GENERATOR BITS
MODE	WGM02	WGM01	WGM00	 DESCRIPTION	 	TOP
0	    0 	    0	    0	     Normal 	 	    0xFF
1	    0	    0	    1	     PWM, Phase Corrected	0xFF
2	    0	    1	    0	     CTC			OCR0A
3	    0	    1	    1	     Fast PWM		0xFF
4	    1	    0	    0	     Reserved	 	-
5	    1	    0	    1	     Fast PWM, Phase Corr	OCR0A
6	    1	    1	    0	     Reserved		-
7	    1	    1	    1	     Fast PWM		OCR0A

MODE	WGM13	WGM12	WGM11	WGM10	 DESCRIPTION            	 TOP
0	0	0 	0	0	 Normal 	                 0xFFFF
1	0	0	0	1	 PWM, Phase Corrected, 8bit	 0x00FF
2	0	0	1	0	 PWM, Phase Corrected, 9bit	 0x01FF
3	0	0	1	1	 PWM, Phase Corrected, 10bit 	 0x03FF 
4	0	1	0	0        CTC	                         OCR1A 
5	0	1	0	1	 Fast PWM, 8bit 	          0x00FF 
6	0	1	1	0	 Fast PWM, 9bit 	          0x01FF 
7	0	1	1	1	 Fast PWM, 10bit 	          0x03FF 
8	1	0	0	0	 PWM, Phase and Frequency Corr    ICR1 
9	1	0	0	1	 PWM, Phase and Frequency Corr    OCR1A 
10	1	0	1	0	 PWM, Phase Correct 	          ICR1 
11	1	0	1	1	 PWM, Phase Correct 	         OCR1A
12	1	1	0	0	 CTC	                         ICR1
13	1	1	0	1	 RESERVED	 
14	1	1	1	0	 Fast PWM 	                  ICR1 
15	1	1	1	1	 Fast PWM	                  OCR1A 

MODE	WGM21	WGM20	 DESCRIPTION	          TOP
0	0	0	 Normal 	         0xFF
1	0	1	 PWM Phase Corrected	 
2	1	0	 CTC	                  OCR2
3	1	1	 Fast PWM 	 



x = Timer Number
7 bit	 6 bit 	 5 bit 	 4 bit 	 3 bit 	 2 bit 	 1 bit 	 0 bit     Description
TCCRxA	COMxA1	 COMxA0  COMxB1  COMxB0  -	 -	 WGMx1	 WGMx0     Timer/Counter Control Register x A (x=0,2)

TCCR1B	ICNC1	 ICES1	 -	 WGM13	 WGM12	 CS12	 CS11	 CS10 
TCCRxB	FOCxA    FOCxB   -   -       WGMx2   CSx2    CSx1    CSx0      Timer/Counter Control Register x B

TIMSKx	-        -       -   -       -       OCIExB  OCIExA  TOIEx     Timer/Counter Interrupt Mask Register
TIFRx	-	     -       -	 -       -       OCFxB	 OCFxA   TOVx      Timer/Counter Interrupt Flag Register
TCNTx                                                                      Timer/Counter Register (stores the counter value)
OCRxA                                                                      Output Compare Register x A
OCRxB                                                                      Output Compare Register x B


*/

#define DEFINE_256 1
//#define DEFINE_512 1


volatile uint8_t sin_tab[N_SIN];

uint8_t mot_A = 0, mot_B = 0, mot_C = 0;

uint8_t pwm_wide= 200;/* max amplitude of PWM sin */
volatile uint16_t position =0;
int freqCounter=0;

void motor_control_init(void) 
{
	/* PWM1 out, PD3. */
	//DDRD |= 0b00001000;
	DDRD |= (1<< DDD3);
	/* PWM2 out, PD5. */
	DDRD |= (1<< DDD5);  
	/* PWM3 out, PD6. */
	DDRD |= (1<< DDD6);  

//#define _BV(bit) (1 << (bit))
 
  TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM00); 
  TCCR0B = _BV(CS00);
  //TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);//COM1A1=1 COM1A0=0 COM1B1=1 COM1B0=0 
  TCCR1A = _BV(WGM10);                            //Clear OC1A/OC1B on Compare Match (Set output to low level).
												  //WGM10=1 PWM, Phase Correct, 8-bit
												  //WGM10=1 WGM11=1 PWM, Phase Correct, 10-bit
  TCCR1B = _BV(CS10);// CS10 =1 - clk/1 no prescaler
  TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
  TCCR2B = _BV(CS20);


  TIMSK1 |= _BV(TOIE1);
  sei();

  // Enable Timer1 Interrupt for Motor Control
  OCR2B = 0;  //D3
  OCR0A = 0;  //D6
  OCR0B = 0;  //D5 

  calc_tab();
}


void calc_sinus_tab(uint8_t max_pwm)
{
  for(int i=0; i<N_SIN; i++)
  {
    #ifdef DEFINE_512
		sin_tab[i] = max_pwm / 2.0 + sin(1.0 * i / N_SIN * 3.14159265) * max_pwm / 2.0;
	#endif
	
	#ifdef DEFINE_256
		sin_tab[i] = max_pwm / 2.0 + sin(2.0 * i / N_SIN * 3.14159265) * max_pwm / 2.0;
	#endif
  }
}

void calc_tab()
{

  cli();
  calc_sinus_tab(pwm_wide);
  sei();
}



void motor_position(int motor_pos)
{

  uint16_t posStep;
  
  posStep = motor_pos;// >> 4 ;//>>3;
  
  #ifdef DEFINE_512
	posStep &= 0x1ff;
  #endif
  
  #ifdef DEFINE_256
	posStep &= 0xff;
  #endif

 
  /* for stem 0-511 */
#if DEFINE_512
  PWM_A_MOTOR0 = get_sin_value(0x1ff & posStep);
  PWM_B_MOTOR0 = get_sin_value(0x1ff &(posStep + 171));
  PWM_C_MOTOR0 = get_sin_value(0x1ff &(posStep + 341));
#endif

#if DEFINE_256
    mot_A = sin_tab[(uint8_t) posStep];
    mot_B = sin_tab[(uint8_t)(posStep + 85)];
    mot_C = sin_tab[(uint8_t)(posStep + 170)];
    
    PWM_A_MOTOR0 = mot_A;
    PWM_B_MOTOR0 = mot_B;
    PWM_C_MOTOR0 = mot_C;
    
//  PWM_A_MOTOR0 = sin_tab[(uint8_t) posStep];
//  PWM_B_MOTOR0 = sin_tab[(uint8_t)(posStep + 85)];
//  PWM_C_MOTOR0 = sin_tab[(uint8_t)(posStep + 170)];
#endif
}

uint8_t get_sin_value( uint16_t pos)
{
#if DEFINE_512
	if ( pos < 256)
	{
		return sin_tab[pos];
	} 
	else 
	{
		return (pwm_wide - sin_tab[pos-256]);
	}
#endif

#if DEFINE_256
	return sin_tab[pos];
#endif

}



/********************************/
/* Motor Control IRQ Routine    */
/********************************/

ISR( TIMER1_OVF_vect )
{
//	cli();
	
//    led_set(LED_RED);
//	freqCounter++;
//	if (freqCounter >= 256)
//	{    
//		freqCounter=0;
		 motor_position(position);
//	}
//	led_clear(LED_RED);
//    sei();
    
}


